// --------------------------------------------------------------------------------------------------------------------
// <copyright file="HomogeneousTransform2DTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test the HomogeneousTransform2D class
    /// </summary>
    [TestClass]
    public class HomogeneousTransform2DTest
    {
        /// <summary>
        /// Test creation of a matrix with rotation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromRotationShouldWorkProperly()
        {
            double angle = 30 * MathConstants.Degrees2Radians;
            double dx = 0;
            double dy = 0;
            double scale = 1;
            Matrix3 ht = HomogeneousTransform2D.CreateFromRotation(angle);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);
        }

        /// <summary>
        /// Test creation of a matrix with translation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromTranslationShouldWorkProperly()
        {
            double angle = 0 * MathConstants.Degrees2Radians;
            double dx = 5.0;
            double dy = 8.0;
            double scale = 1;
            Matrix3 ht = HomogeneousTransform2D.CreateFromTranslation(dx, dy);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);
        }

        /// <summary>
        /// Test creation of a matrix with rotation and translation
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromRotationAndTranslationShouldWorkProperly()
        {
            double angle = 0 * MathConstants.Degrees2Radians;
            double dx = 5.0;
            double dy = 8.0;
            double scale = 1;
            Matrix3 ht = HomogeneousTransform2D.CreateFromRotationAndTranslation(angle, dx, dy);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);
        }

        /// <summary>
        /// Test creation of a matrix from the orthogonal $Procrustes$ algorithm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromProcrustesShouldWorkProperlyWithRotation()
        {
            // ground truth
            double angle = -90 * MathConstants.Degrees2Radians;
            double dx = 0;
            double dy = 0;
            double scale = 1;

            // Create equilateral triangle centered at origin, oriented vertically up 
            double triangleSideLength = 10;
            double triangleHeight = triangleSideLength * Math.Sqrt(3) / 2;
            Vector2[] points1 = new Vector2[3];
            points1[0] = new Vector2(0, 2 * triangleHeight / 3);
            points1[1] = new Vector2(-triangleSideLength / 2, -triangleHeight / 3);
            points1[2] = new Vector2(triangleSideLength / 2, -triangleHeight / 3);

            // Create equilateral triangle centered at origin, oriented to the right 
            Vector2[] points2 = new Vector2[3];
            points2[0] = new Vector2(2 * triangleHeight / 3, 0);
            points2[1] = new Vector2(-triangleHeight / 3, triangleSideLength / 2);
            points2[2] = new Vector2(-triangleHeight / 3, -triangleSideLength / 2);

            // Check matrix
            Matrix3 ht = HomogeneousTransform2D.CreateFromProcrustes(points1, points2, false, false);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);

            // Check points transformed by matrix 
            for (int i = 0; i < 3; i++)
            {
                Vector2 pointOut = HomogeneousTransform2D.ApplyAffine(ht, points1[i]);
                Assert.IsTrue(pointOut.Equals(points2[i]));
            }
        }

        /// <summary>
        /// Test creation of a matrix from the orthogonal $Procrustes$ algorithm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromProcrustesShouldWorkProperlyWithRotationAndTranslation()
        {
            // ground truth
            double angle = -90 * MathConstants.Degrees2Radians;
            double dx = 5;
            double dy = 0;
            double scale = 1;

            // Create equilateral triangle centered at origin, oriented vertically up 
            double triangleSideLength = 10;
            double triangleHeight = triangleSideLength * Math.Sqrt(3) / 2;
            Vector2[] points1 = new Vector2[3];
            points1[0] = new Vector2(0, 2 * triangleHeight / 3);
            points1[1] = new Vector2(-triangleSideLength / 2, -triangleHeight / 3);
            points1[2] = new Vector2(triangleSideLength / 2, -triangleHeight / 3);

            // Create equilateral triangle centered at origin, oriented to the right 
            Vector2[] points2 = new Vector2[3];
            points2[0] = new Vector2(2 * triangleHeight / 3 + dx, 0);
            points2[1] = new Vector2(-triangleHeight / 3 + dx, triangleSideLength / 2);
            points2[2] = new Vector2(-triangleHeight / 3 + dx, -triangleSideLength / 2);

            // Check matrix
            Matrix3 ht = HomogeneousTransform2D.CreateFromProcrustes(points1, points2, false, false);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);

            // Check points transformed by matrix 
            for (int i = 0; i < 3; i++)
            {
                Vector2 pointOut = HomogeneousTransform2D.ApplyAffine(ht, points1[i]);
                Assert.IsTrue(pointOut.Equals(points2[i]));
            }
        }

        /// <summary>
        /// Test creation of a matrix from the $Procrustes$ algorithm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromProcrustesShouldWorkProperlyWithScale()
        {
            // ground truth
            double angle = 0;
            double dx = 0;
            double dy = 0;
            double scale = 2;

            // Create equilateral triangle centered at origin, oriented vertically up 
            double triangleSideLength = 10;
            double triangleHeight = triangleSideLength * Math.Sqrt(3) / 2;
            Vector2[] points1 = new Vector2[3];
            points1[0] = new Vector2(0, 2 * triangleHeight / 3);
            points1[1] = new Vector2(-triangleSideLength / 2, -triangleHeight / 3);
            points1[2] = new Vector2(triangleSideLength / 2, -triangleHeight / 3);

            // Create equilateral triangle centered at origin, oriented vertically up, scaled 
            Vector2[] points2 = new Vector2[3];
            points2[0] = new Vector2(0 * scale, 2 * triangleHeight / 3 * scale);
            points2[1] = new Vector2(-triangleSideLength / 2 * scale, -triangleHeight / 3 * scale);
            points2[2] = new Vector2(triangleSideLength / 2 * scale, -triangleHeight / 3 * scale);

            // Check matrix
            Matrix3 ht = HomogeneousTransform2D.CreateFromProcrustes(points1, points2, true, false);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);

            // Check points transformed by matrix 
            for (int i = 0; i < 3; i++)
            {
                Vector2 pointOut = HomogeneousTransform2D.ApplyAffine(ht, points1[i]);
                Assert.IsTrue(pointOut.Equals(points2[i]));
            }
        }

        /// <summary>
        /// Test creation of a matrix from the $Procrustes$ algorithm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromProcrustesShouldWorkProperlyWithRotationAndScale()
        {
            // ground truth
            double angle = -90 * MathConstants.Degrees2Radians;
            double dx = 0;
            double dy = 0;
            double scale = 2;

            // Create equilateral triangle centered at origin, oriented vertically up 
            double triangleSideLength = 10;
            double triangleHeight = triangleSideLength * Math.Sqrt(3) / 2;
            Vector2[] points1 = new Vector2[3];
            points1[0] = new Vector2(0, 2 * triangleHeight / 3);
            points1[1] = new Vector2(-triangleSideLength / 2, -triangleHeight / 3);
            points1[2] = new Vector2(triangleSideLength / 2, -triangleHeight / 3);

            // Create equilateral triangle centered at origin, oriented to the right, scaled 
            Vector2[] points2 = new Vector2[3];
            points2[0] = new Vector2(2 * triangleHeight / 3 * scale, 0 * scale);
            points2[1] = new Vector2(-triangleHeight / 3 * scale, triangleSideLength / 2 * scale);
            points2[2] = new Vector2(-triangleHeight / 3 * scale, -triangleSideLength / 2 * scale);

            // Check matrix
            Matrix3 ht = HomogeneousTransform2D.CreateFromProcrustes(points1, points2, true, false);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);

            // Check points transformed by matrix 
            for (int i = 0; i < 3; i++)
            {
                Vector2 pointOut = HomogeneousTransform2D.ApplyAffine(ht, points1[i]);
                Assert.IsTrue(pointOut.Equals(points2[i]));
            }
        }

        /// <summary>
        /// Test creation of a matrix from the $Procrustes$ algorithm
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void CreationFromProcrustesShouldWorkProperlyWithRotationAndTranslationAndScale()
        {
            // ground truth
            double angle = -90 * MathConstants.Degrees2Radians;
            double dx = 5;
            double dy = 0;
            double scale = 2;

            // Create equilateral triangle centered at origin, oriented vertically up 
            double triangleSideLength = 10;
            double triangleHeight = triangleSideLength * Math.Sqrt(3) / 2;
            Vector2[] points1 = new Vector2[3];
            points1[0] = new Vector2(0, 2 * triangleHeight / 3);
            points1[1] = new Vector2(-triangleSideLength / 2, -triangleHeight / 3);
            points1[2] = new Vector2(triangleSideLength / 2, -triangleHeight / 3);

            // Create equilateral triangle centered at origin, oriented to the right, scaled 
            Vector2[] points2 = new Vector2[3];
            points2[0] = new Vector2((2 * triangleHeight / 3 + dx) * scale, 0 * scale);
            points2[1] = new Vector2((-triangleHeight / 3 + dx) * scale, triangleSideLength / 2 * scale);
            points2[2] = new Vector2((-triangleHeight / 3 + dx) * scale, -triangleSideLength / 2 * scale);

            // Check matrix
            Matrix3 ht = HomogeneousTransform2D.CreateFromProcrustes(points1, points2, true, false);
            this.TestHomogeneousMatrixAgainstGroundTruth(ht, angle, dx, dy, scale);

            // Check points transformed by matrix 
            for (int i = 0; i < 3; i++)
            {
                Vector2 pointOut = HomogeneousTransform2D.ApplyAffine(ht, points1[i]);
                Assert.IsTrue(pointOut.Equals(points2[i]));
            }
        }

        /// <summary>
        /// Compares the homogeneous matrix against ground truth and fires an assertion if they are not equal
        /// </summary>
        /// <param name="ht">the homogeneous matrix</param>
        /// <param name="groundTruthAngle">the ground truth angle</param>
        /// <param name="groundTruthDx">the ground truth displacement in x</param>
        /// <param name="groundTruthDy">the ground truth displacement in y</param>
        /// <param name="groundTruthScale">the ground truth scale</param>
        private void TestHomogeneousMatrixAgainstGroundTruth(Matrix3 ht, double groundTruthAngle, double groundTruthDx, double groundTruthDy, double groundTruthScale)
        {
            double cosAngle = Math.Cos(groundTruthAngle);
            double sinAngle = Math.Sin(groundTruthAngle);

            Assert.AreEqual(groundTruthScale * cosAngle, ht[0, 0], MathConstants.ErrorEpsilon);
            Assert.AreEqual(-groundTruthScale * sinAngle, (ht[0, 1]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(groundTruthScale * groundTruthDx, (ht[0, 2]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(groundTruthScale * sinAngle, (ht[1, 0]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(groundTruthScale * cosAngle, (ht[1, 1]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(groundTruthScale * groundTruthDy, (ht[1, 2]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(0.0, (ht[2, 0]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(0.0, (ht[2, 1]), MathConstants.ErrorEpsilon);
            Assert.AreEqual(1.0, (ht[2, 2]), MathConstants.ErrorEpsilon);
        }
    }
}
